#include "modules/CtrlModule/ActivePlanner/VirtualObsFollowPlanner.h"
#include "modules/CtrlModule/ActivePlanner/NaviPlanner.h"
#include "lib/Time.h"

namespace behavior_controller
{
    #define OBS_DISTANCE 0.06 //需要控制的与障碍物的距离
    #define MAX_DISTANCE 0.5  //沿边传感器的最大距离
    VirtualObsFollowPlanner virtual_obs_follow_planner;
    void VirtualObsFollowPlanner::run(double &linear_vel, double &angular_vel,bool &finished)
    {
        //linear为正向前，angular为正向左
        std::cout<< "run dis_ : " <<dis_<<std::endl;
        if (dis_ >= MAX_DISTANCE) //右侧无障碍物的情况右转
        {
            linear_vel = 0.2;
            angular_vel = -0.8;
            last_dis_p=0;
        }
        else if (dis_ < MAX_DISTANCE && dis_ >= (OBS_DISTANCE+0.04)) //右侧有障碍物，但距离还有些远的情况
        {
            linear_vel = 0.2;
            angular_vel = -0.6;
            last_dis_p=0;
        }
        else if (dis_ >= 0.005 && dis_ < (OBS_DISTANCE+0.04)) //沿边时需要调整的情况
        {
            pid_adjustment(linear_vel, angular_vel);
        }
        else
        {
            linear_vel = 0.2;
            angular_vel = 0.2;
            last_dis_p=0;
        }
        if( (pow(DataSet::robot_pose.x() -start_pose.pose.position.x,2)+
                    pow(DataSet::robot_pose.y()-start_pose.pose.position.y,2))<= 0.09 
                    && Time::Now()-start_pose.header.stamp.sec> 120 )  // 120s   , 0.3m -> 0.3*0.3=0.09
        {
            finished=true;
        }
    }

    void VirtualObsFollowPlanner::pid_adjustment(double &linear_vel, double &angular_vel)
    {
        linear_vel = 0.2;

        double dis_p = dis_ - OBS_DISTANCE;       //当dis_p>0时，应向右转
        double dis_d = dis_p - last_dis_p;        //当dis_d>0时，说明变化趋势在增大，应反作用于角速度
        angular_vel = -dis_p * 2.7 - dis_d * 0.3; //角速度输出，需要调节p d参数，使机器人行走轨迹不至于过于晃动。
        last_dis_p = dis_p;
    }

} // namespace behavior_controller